#include "CPrimCone.h"
#include <math.h>

CPrimCone::CPrimCone(float radius, float height):IPrimitive(),
	m_radius(radius), m_height(height)
{
	m_angle = atan(m_radius / m_height);
	m_tanAngle = m_radius / m_height;
	m_tanAnglePowerTwo = m_tanAngle * m_tanAngle;
}

CPrimCone::~CPrimCone(){}


bool CPrimCone::intersect(const CRay& ray, float& dist, bool useGlobalCoordinates)
{
	//change the coordinate system
	CRay modRay;
	if (useGlobalCoordinates)
		globalToLocal(ray, modRay);
	else
		convertRayToLocalCoordinates(ray, modRay);

	//calulate A B and C in At^2 + Bt + C = 0
	//it comes from the cone formula : x^2 + z^2 = y^2 * tan(a)^2

	float A = modRay.getDirection().x * modRay.getDirection().x + modRay.getDirection().z * modRay.getDirection().z -
		modRay.getDirection().y * modRay.getDirection().y * m_tanAnglePowerTwo;

	float B = 2 * (modRay.getDirection().x * modRay.getOrigin().x + modRay.getDirection().z * modRay.getOrigin().z - 
		modRay.getDirection().y * modRay.getOrigin().y * m_tanAnglePowerTwo);

	float C = modRay.getOrigin().x * modRay.getOrigin().x + modRay.getOrigin().z * modRay.getOrigin().z - 
		modRay.getOrigin().y * modRay.getOrigin().y * m_tanAnglePowerTwo;

	//calculate the solution ot the equation
	float delta = B * B - 4 * A * C;

	//for the degenerate point
	if(delta > -0.001f && delta < 0.001f)
		delta = 0;

	if(delta > 0)
	{
		float sqrtDelta = sqrt(delta);

		float t1 = (-B + sqrtDelta) / (2 * A);
		float t2 = (-B - sqrtDelta) / (2 * A);

		float H1 = modRay.getOrigin().y + modRay.getDirection().y * t1;
		float H2 = modRay.getOrigin().y + modRay.getDirection().y * t2;

		if(!isValideHeight(H1) && !isValideHeight(H2))
			return false;

		if(isValideHeight(H1) && isValideHeight(H2))
		{
			if(t1 <= 0 && t2 <= 0)
				return false;
			else if(t1 <= 0)
				dist = t2;
			else if(t2 <= 0)
				dist = t1;
			else
				dist = fmin(t1, t2);
			return true;
		}
		else if(isValideHeight(H1))
			dist = t1;
		else 
			dist = t2;

		//check for the top end
		float tEnd = calculateTTopEnd(modRay);
		if(tEnd < dist && tEnd > 0)
			dist = tEnd;

		if(dist < 0)
			return false;

		return true;

	}
	else if (delta == 0)
	{
		//top end
		float tEnd = calculateTTopEnd(modRay);

		//tangent to the cone
		dist = -B / (2 * A);
		
		if(tEnd < dist && tEnd > 0)
			dist = tEnd;

		float H = modRay.getOrigin().y + modRay.getDirection().y * dist;
		if(!isValideHeight(H))
			return false;

		return true;
	}
	return false;
}

void CPrimCone::computeNormal(const CRay& ray, float dist, CVector3f& normal)const
{
	//change the coordinate system
	CRay R;
	convertRayToLocalCoordinates(ray, R);

	//calculate intersection point
	CVector3f intersection = R.getOrigin() + R.getDirection() * dist;

	//if close from 0
	if(intersection.y < 0.001f && intersection.y > -0.001f)
		intersection.y = 0;

	//if valid height
	if(intersection.y + 0.001 < m_height && intersection.y != 0)
	{
		normal = intersection;
		normal.y = 0;
		normal.normalize();

		//rotation axis
		CVector3f R;

		if(normal.x != 0)
			if(normal.z < 0 && normal.x < 0) //both negative
				R = CVector3f(normal.z/normal.x, 0, -1);
			else if(normal.z < 0) // z negative and x positive
				R = CVector3f(-normal.z/normal.x, 0, 1);
			else if(normal.x < 0) //z positif and x negative
				R = CVector3f(normal.z/normal.x, 0, -1);
			else //both positive
				R = CVector3f(-normal.z/normal.x, 0, 1);
		else
			if(normal.z < 0)
				R = CVector3f(1, 0, 0);
			else
				R = CVector3f(-1, 0, 0);

		R.normalize();

		//create a matrix rotation
		CMatrix rotationMatrix;
		rotationMatrix.createRotation(R, m_angle/2);

		normal = normal * rotationMatrix;
	}
	else if (intersection.y == 0)
		normal = CVector3f(0, -1, 0);
	else
		normal = CVector3f(0, 1, 0);

	//if ray from the inside
	if(R.getDirection().dot(normal) > 0)
		normal = normal * -1;

	//reset to the global coordinate systeme
	convertNormalToGlobalCoordinates(normal, normal);


}

bool CPrimCone::isInside(const CVector3f& p)const
{
	//convert p to local coordinates
	CVector3f localP;
	convertPointToLocalCoordinates(p, localP);

	//check if localP is high enough
	if(!isValideHeight(localP.y))
		return false;

	//get the radius at the specific height
	double radius = m_tanAnglePowerTwo * localP.y * localP.y;

	//check if inside the circle
	if(localP.x * localP.x + localP.z * localP.z <= radius)
		return true;

	return false;

}

bool CPrimCone::isValideHeight(float h)const
{
	if(h >= 0 && h <= m_height)
		return true;

	return false;
}

float CPrimCone::fmin(float f1, float f2)const
{
	if(f1 < f2)
		return f1;

	return f2;
}

float CPrimCone::calculateTTopEnd(const CRay& R)const
{
	return (m_height - R.getOrigin().y)/R.getDirection().y;
}